/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : position_ctrl.hpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月24日
  ******************************************************************************
  */
/* USER CODE END Header */

#ifndef __POSITION_CTRL_HPP
#define __POSITION_CTRL_HPP

#include "userlib/userlib.hpp"
#include "system/system.hpp"
#include "control/pid/pid.hpp"

#ifdef __cplusplus
extern "C" {
#endif

typedef enum
{
	QAUD_MANUAL		= 0x00U,	//手控
	QAUD_ATTITUDE	= 0x01U,	//姿态
	QAUD_VEL_HOLD	= 0x02U,	//控速
	QAUD_POS_HOLD	= 0x03U,	//定点
	QAUD_TRAJECTORY	= 0x04U,	//航线
}eQUAD_MODE;

#ifdef __cplusplus
}
#endif


#ifdef __cplusplus

using namespace std;

class POSITION_CTRL
{
public:
	POSITION_CTRL();
	~POSITION_CTRL();
	void Position_Ctrl_Init(void);
	void Position_Ctrl_Run(void);
	void flight_Mode_Update(void);
	void takeOff_Land_check(void);

	void pid_Para_Update();
private:

	float SetXyz[3];
	float SetUvw[3];
	float SetAcc[3];
	float FeedBackXyz[3];
	float FeedBackUvw[3];

	float dt;
	uint8_t state;	//飞机状态：飞行状态、待起飞状态、降落状态
	bool isTakeOff;
	bool isLand;

	u8 pid_Para_Update_Time;
	eQUAD_MODE Mode;
	uint8_t isPosHold;
	uint8_t isAltHold;
	float OneG;
	float u1_Bar;
	float Pd_Bar;
	float Rd_Bar;
	float u1_Tilt;

	float Z_OUT;
	float HOVER_PWM;

	float thrust_command;
	float roll_command;
	float pitch_command;

	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;
	uint32_t startTimerLast;
	uint32_t cycleTime_us;

	Pid_msg pid_para;
	mahonyAhrs_msg mahonyAhrs_Ang;
	RC_command_msg rcCommand;
	flow_msg flow;
	height_msg height;
	sensor_acc_msg acc;
	Inloop_control_msg inloop_control;
	ekf_uwb_pos_vel_msg ekf_uwb_pos_vel;
};


#endif


#endif

/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
